# -*- coding: utf-8 -*-

import numpy as np
import time
import PX4MavCtrlV4 as PX4MavCtrl



# 控制飞机起飞到一定高度
VehilceNum = 5
MavList = []

host_ip = "127.0.0.1"

# Create MAV instance
mavL = [PX4MavCtrl.PX4MavCtrler(20100+i*2, host_ip) for i in range(VehilceNum)]
time.sleep(2)


# Start MAV loop with UDP mode: MAVLINK_FULL


for mav in mavL:
    mav.InitMavLoop()
    mav.InitTrueDataLoop()

# Enter Offboard mode to start vehicle control
time.sleep(2)
for mav in mavL:
    mav.initOffboard()

# Get the takeoff position of each vehicle to the UE4 Map
# this can be adopted to obtain the global position of a vehicle in swarm simulation
time.sleep(2)


# fly to 10m high above its takeoff position
for mav in mavL:
    mav.SendPosNED(0, 0, -10, 0)
    mav.SendCopterSpeed(3)
time.sleep(10)

for mav in mavL:
    mav.SendPosNED(100, 0, -10, 0)
    mav.SendCopterSpeed(0.5)
time.sleep(10)


